#include <SoftwareSerial.h>
#include <L298Driver.h>

L298Driver motors(5,7,8,10,6,9);
int BT_RXpin=12;
int BT_TXpin=13;

SoftwareSerial BTSerial(BT_RXpin,BT_TXpin);
char val;
uint8_t motorOn;


void help() {
  BTSerial.print("- L298Motor+BT v1.0 - (c) 2013 Mauricio M Costa\n\r");
  BTSerial.print("Navigation:\n\r");
  BTSerial.print("   |  E  |  T\n\r");
  BTSerial.print("A  |S   D|  H\n\r");
  BTSerial.print("Z  |  X  |   \n\r");
  BTSerial.println();
  BTSerial.print("- S D: turns right / left\n\r");
  BTSerial.print("- E X: moves forward / backward\n\r");
  BTSerial.print("- T H: test mode / this help\n\r");
  BTSerial.print("- A Z: toggle engine on-off / brake\n\r");


}

void test() {
	// motor off
	motors.Off(L298_A_CHANNEL| L298_B_CHANNEL);
	motors.Set(L298_A_CHANNEL,L298_BRAKE1);
	motors.Set(L298_B_CHANNEL,L298_BRAKE1);
	delay(2000);
	// blink ENABLE pins
	motors.On(L298_A_CHANNEL| L298_B_CHANNEL);
	delay(1000);
	motors.Off(L298_A_CHANNEL| L298_B_CHANNEL);
	delay(1000);
	motors.On(L298_A_CHANNEL| L298_B_CHANNEL);
	delay(1000);
	motors.Off(L298_A_CHANNEL| L298_B_CHANNEL);
	delay(2000);
	// blink line pins
	for(uint8_t i=0;i<4;i++) {
		motors.Set(L298_A_CHANNEL,i);
		motors.Set(L298_B_CHANNEL,i);
		delay(1000);
	}
}

void setup() {
  motors.Off(L298_A_CHANNEL|L298_B_CHANNEL);
  motorOn=0;

  BTSerial.begin(9600);
  help();  
}

void loop() {

  if(BTSerial.available()) {
    val=BTSerial.read();
    val=toupper(val);
  }
  switch(val) {
  case 'S':
    motors.Set(L298_A_CHANNEL,2);
    motors.Set(L298_B_CHANNEL,1);
    break;
  case 'D':
    motors.Set(L298_A_CHANNEL,1);
    motors.Set(L298_B_CHANNEL,2);
    break;
  case 'E':
    motors.Set(L298_A_CHANNEL,1);
    motors.Set(L298_B_CHANNEL,1);
    break;
  case 'X':
    motors.Set(L298_A_CHANNEL,2);
    motors.Set(L298_B_CHANNEL,2);
    break;
  case 'T':
    test();
    break;
  case 'Z':
    motors.Set(L298_A_CHANNEL,0);
    motors.Set(L298_B_CHANNEL,0);
    motors.Off(L298_A_CHANNEL|L298_B_CHANNEL);
    break;
  case 'H':
    motors.Off(L298_A_CHANNEL|L298_B_CHANNEL);
    help();
    val=0;
    break;
  case 'A':
    if(motorOn==1) {
      motorOn=0;
      motors.Off(L298_A_CHANNEL|L298_B_CHANNEL);
    } 
    else {
      motorOn=1;
      motors.On(L298_A_CHANNEL|L298_B_CHANNEL);
    }
    val=0;
    break;
  }
}






